/**
 *@file foc.c
 * @author zyx
 * @brief
 * @version 0.1
 * @date 2024-05-06
 *
 * @copyright Copyright (c) 2024
 *
 */
#include "main.h"
#include "foc.h"
#include "usart.h"
#include <stdio.h>
#include "arm_math.h"
#include "mt6816.h"
#include "pi.h"
#define Udc 20
#include "usbd_cdc_if.h"
// extern TIM_HandleTypeDef htim2;
// extern ADC_HandleTypeDef hadc1;
FOC_parame_ foc_curren = {
	.Kp = 0.0001f, // 设置 Kp 的初始值
	.Ki = 0.0001f  // 设置 Ki 的初始值
};

/**
 * @brief  Ƶ�ʣ�PWM_FRQ= 15KHZ
 * @param[in,out]
 * @param[in]
 * AMP_IU(adc�ɼ�ֵ) = 6 * ( 0.02*I ) + 1.25V��
 */

/**
 * @brief  Ƶ�ʣ�PWM_FRQ= 15KHZ  adc�жϻص�����
 * @param[in,out]
 * @param[in]
 */
float theta1;
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *hadc)
{
	foc_curren.Ud = 0;
	foc_curren.Uq = 0;
	static uint16_t adcvalue[2];
	static float theta;
	/*1.调0后获取电气角度*/
	theta1 = get_angle_M();
	if (theta < 0)
	{
		theta += 360.0f;
	}

	foc_curren.theta = 0; // theta * 0.1221730476392222f;
	/*2.电流获取*/
	//	adcvalue[0] = hadc->Instance->JDR1;
	//	adcvalue[1] = hadc->Instance->JDR2;
	//	foc_curren.Ia = ((adcvalue[0] ) * 3.3f / 4096.0f -1.65) / RES;
	//	foc_curren.Ib = ((adcvalue[1] ) * 3.3f / 4096.0f-1.65) / RES;
	//	 /*3.克拉克变换*/
	//	 Clarke(&foc_curren);
	//	 /*4.帕克变换*/
	//	 Park(&foc_curren);
	//	 /*5.电流PI*/
	//	 PI_Controller_Uq(&foc_curren, 0);
	//	 PI_Controller_Ud(&foc_curren, 1);
	/*6.反park变换*/
	R_park(&foc_curren);
	/*7.SVPWM*/
	SVPWM(&foc_curren);
	TIM1->CCR1 = foc_curren.Ta;
	TIM1->CCR2 = foc_curren.Tb;
	TIM1->CCR3 = foc_curren.Tc;
	// usb_printf("%.2f,%.2f,%.2f\n",foc_curren.Ta,foc_curren.Tb,foc_curren.Tc);
}
// void printf_u(void){
//	usb_printf("%.2f,%.2f,%.2f\n",foc_curren.Ta,foc_curren.Tb,foc_curren.Tc);
// }
void Control(void)
{
	// foc_curren.Uq = 0.5f;
	// foc_curren.Ud = 0;
	// // foc_curren.theta = get_angle_M() * (M_PI / 180.0f) * 7; // 7������
	// foc_curren.theta = 0;
	// R_park(&foc_curren);
	// SVPWM(&foc_curren);
	// TIM1->CCR1 = foc_curren.Ta;
	// TIM1->CCR2 = foc_curren.Tb;
	// TIM1->CCR3 = foc_curren.Tc;
	// // printf("%f, %f, %f\n",(float) TIM1->CCR1,(float) TIM1->CCR2,(float) TIM1->CCR3);
}

/**
 * @brief  I����=>Idq
 * @param[in,out]
 * @param[in]
 */
void Park(FOC_parame_ *foc_)
{
	foc_->I_d = foc_->I_alpha * arm_cos_f32(foc_->theta) + foc_->I_beta * arm_sin_f32(foc_->theta);
	foc_->I_q = -foc_->I_alpha * arm_sin_f32(foc_->theta) + foc_->I_beta * arm_cos_f32(foc_->theta);
}
/**
 * @brief  Iabc=>I����
 * @param[in,out]
 * @param[in]
 * ����֤
 */
void Clarke(FOC_parame_ *foc_)
{
	foc_->I_alpha = foc_->Ia;
	foc_->I_beta = (foc_->Ia + 2 * foc_->Ib) / sqrt(3);
}
/**
 * @brief  Udq=>U����
 * @param[in,out]
 * @param[in]
 *  ����֤
 */
void R_park(FOC_parame_ *foc_)
{
	foc_->U_alpha = foc_->Ud * arm_cos_f32(foc_->theta) - foc_->Uq * arm_sin_f32(foc_->theta);
	foc_->U_beta = foc_->Ud * arm_sin_f32(foc_->theta) + foc_->Uq * arm_cos_f32(foc_->theta);
}

/**
 * @brief  U����=>Tabc(TIM1-CCR123)
 * @param[in,out]
 * @param[in]
 * ����֤
 */
void SVPWM(FOC_parame_ *foc_)
{
	static uint8_t sector = 0;
	static float v1, v2, v3, X, Y, Z, T1, T2, Tcmp1, Tcmp2, Tcmp3, ta, tb, tc;
	Tcmp1 = 0;
	Tcmp2 = 0;
	Tcmp3 = 0;
	sector = 0;
	//========Parameters statement================

	v1 = foc_->U_beta;
	v2 = (sqrt(3) * foc_->U_alpha - foc_->U_beta) / 2.0f;
	v3 = (-sqrt(3) * foc_->U_alpha - foc_->U_beta) / 2.0f;
	//========Sector calculation=================

	if (v1 > 0)
		sector = 1;

	if (v2 > 0)
		sector = sector + 2;

	if (v3 > 0)
		sector = sector + 4;
	//======== X Y Z calculation===================
	X = sqrt(3) * foc_->U_beta * PWM_FRQ / Udc;
	Y = PWM_FRQ / Udc * (3 / 2.0f * foc_->U_alpha + sqrt(3) / 2.0f * foc_->U_beta);
	Z = PWM_FRQ / Udc * (-3 / 2.0f * foc_->U_alpha + sqrt(3) / 2.0f * foc_->U_beta);

	//=========Duty ratio calculation================
	switch (sector)
	{
	case 1:
		T1 = Z;
		T2 = Y;
		break;
	case 2:
		T1 = Y;
		T2 = -X;
		break;
	case 3:
		T1 = -Z;
		T2 = X;
		break;
	case 4:
		T1 = -X;
		T2 = Z;
		break;
	case 5:
		T1 = X;
		T2 = -Y;
		break;
	default:
		T1 = -Y;
		T2 = -Z;
		break;
	}
	if (T1 + T2 > PWM_FRQ)
	{
		T1 = T1 / (T1 + T2);
		T2 = T2 / (T1 + T2);
	}
	else
	{
		T1 = T1;
		T2 = T2;
	}

	ta = (PWM_FRQ - (T1 + T2)) / 4.0f;
	tb = ta + T1 / 2;
	tc = tb + T2 / 2;

	//==========Duty ratio calculation================
	switch (sector)
	{
	case 1:
		Tcmp1 = tb;
		Tcmp2 = ta;
		Tcmp3 = tc;
		break;
	case 2:
		Tcmp1 = ta;
		Tcmp2 = tc;
		Tcmp3 = tb;
		break;
	case 3:
		Tcmp1 = ta;
		Tcmp2 = tb;
		Tcmp3 = tc;
		break;
	case 4:
		Tcmp1 = tc;
		Tcmp2 = tb;
		Tcmp3 = ta;
		break;
	case 5:
		Tcmp1 = tc;
		Tcmp2 = ta;
		Tcmp3 = tb;
		break;
	case 6:
		Tcmp1 = tb;
		Tcmp2 = tc;
		Tcmp3 = ta;
		break;
	}

	foc_->Ta = Tcmp1;
	foc_->Tb = Tcmp2;
	foc_->Tc = Tcmp3;
}
